05. Global Kinematic Model

Now we’ve developed equations to determine the next state (state at t+1) from our state vector at t and our actuator values. Note that we’ve added a variable to our state called L_f which measures the distance between the front of the vehicle and its center of gravity. The larger the vehicle , the slower the turn rate.

\large x_{t+1} = x_t + v_t cos(\psi_t) * dt

\large y_{t+1} = y_t + v_t sin(\psi_t) * dt

\large \psi_{t+1} = \psi_t + \frac {v_t} { L_f} \delta * dt

\large v_{t+1} = v_t + a_t * dt

In the quiz below we will implement the global kinematic model to return the next state vector, given a current state input vector and an actuator vector.

Start Quiz:

// In this quiz you'll implement the global kinematic model.
#include <math.h>
#include <iostream>
#include "Dense"

//
// Helper functions
//
double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }

const double Lf = 2;


Eigen::VectorXd globalKinematic(Eigen::VectorXd state,
                                Eigen::VectorXd actuators, double dt) {
  Eigen::VectorXd next_state(state.size());
  
  //TODO: Implement the Global Kinematic Model, to return
  // the next state from inputs

  // NOTE: state is [x, y, psi, v]
  // NOTE: actuators is [delta, a]
  
  //Add your code below

  return next_state;
}

int main() {
  // [x, y, psi, v]
  Eigen::VectorXd state(4);
  // [delta, v]
  Eigen::VectorXd actuators(2);

  state << 0, 0, deg2rad(45), 1;
  actuators << deg2rad(5), 1;

  Eigen::VectorXd next_state = globalKinematic(state, actuators, 0.3);

  std::cout << next_state << std::endl;
}

[x, y, \psi, v] is the state of the vehicle, L_f is a physical characteristic of the vehicle, and [\delta, a] are the actuators, or control inputs, to our system.

This is a good kinematic model!

Now that we have this, we can use the model to write a simulation where we can develop a controller to follow a trajectory.